#include "Devices.h"
 
// Device
int Device::AttachHandler(CPhidgetHandle Handle, void *UserPtr) // Attach handler
{
	#ifdef SHOW_DEBUG
		int SerialNo;
		string *Name;
		CPhidget_getDeviceName (Handle, (const char**)&Name);
		CPhidget_getSerialNumber(Handle, &SerialNo);
		printf("%s %10d attached!\n", Name, SerialNo);
	#endif
	return 0;
};

int Device::DetachHandler(CPhidgetHandle Handle, void *UserPtr) // Detach handler
{
	#ifdef SHOW_DEBUG
		int SerialNo;
		string *Name;
		CPhidget_getDeviceName (Handle, (const char**)&Name);
		CPhidget_getSerialNumber(Handle, &SerialNo);
		printf("%s %10d detached!\n", Name, SerialNo);
	#endif
	return 0;
};

int Device::ErrorHandler(CPhidgetHandle Handle, void *UserPtr, int ErrorCode, const char *Description) // Error handler
{
	#ifdef SHOW_ERROR
		printf("Error handled:\n%d - %s\n",ErrorCode, Description);
	#endif
	return 0;
}

int Device::wait(unsigned long milisec)
{
    struct timespec req={0};
    time_t sec=(int)(milisec/1000);
    milisec=milisec-(sec*1000);
    req.tv_sec=sec;
    req.tv_nsec=milisec*1000000L;
    while(nanosleep(&req,&req)==-1)
         continue;
    return 1;
}

// Interface board
InterfaceBoard::InterfaceBoard() // Constructor
{
	// Create the InterfaceKit Handle
	CPhidgetInterfaceKit_create(&Handle);

	// Assign callbacks/handlers
	CPhidget_set_OnAttach_Handler((CPhidgetHandle)Handle, AttachHandler, this);
	CPhidget_set_OnDetach_Handler((CPhidgetHandle)Handle, DetachHandler, this);
	CPhidget_set_OnError_Handler((CPhidgetHandle)Handle, ErrorHandler, this);
	CPhidgetInterfaceKit_set_OnInputChange_Handler (Handle, InputChangeHandler, this);
	//CPhidgetInterfaceKit_set_OnSensorChange_Handler (Handle, SensorChangeHandler, this);
	CPhidgetInterfaceKit_set_OnOutputChange_Handler (Handle, OutputChangeHandler, this);
	//CPhidget_enableLogging(PHIDGET_LOG_VERBOSE,NULL);
	Sensors[0] = new SENSOR_SLOT_0(Handle);
	Sensors[1] = new SENSOR_SLOT_1(Handle);
	Sensors[2] = new SENSOR_SLOT_2(Handle);
	Sensors[3] = new SENSOR_SLOT_3(Handle);
	Sensors[4] = new SENSOR_SLOT_4(Handle);
	Sensors[5] = new SENSOR_SLOT_5(Handle);
	Sensors[6] = new SENSOR_SLOT_6(Handle);
	Sensors[7] = new SENSOR_SLOT_7(Handle);
};

InterfaceBoard::~InterfaceBoard() // Destructor
{
	for(int i=0;i<8;i++) delete Sensors[i];
	CPhidget_delete((CPhidgetHandle)Handle);
};

int InterfaceBoard::Open(int SerialNumber) // Open new connection
{
	int result;
	string *err;
	CPhidget_open((CPhidgetHandle)Handle, SerialNumber);
	//wait(500);
	// Wait for attachment
	if((result = CPhidget_waitForAttachment((CPhidgetHandle)Handle, 5000)))
	{
		// Print error message after time out
		CPhidget_getErrorDescription(result, (const char**)&err);
		#ifdef SHOW_ERROR
		printf("Problem waiting for attachment: %s\n", err);
		#endif
		return 0;
	}
	CPhidgetInterfaceKit_setDataRate(Handle, 0, 64);
	CPhidgetInterfaceKit_setDataRate(Handle, 1, 64);
	CPhidgetInterfaceKit_setDataRate(Handle, 2, 64);
	CPhidgetInterfaceKit_setDataRate(Handle, 3, 64);
	CPhidgetInterfaceKit_setDataRate(Handle, 4, 64);
	CPhidgetInterfaceKit_setDataRate(Handle, 5, 64);
	CPhidgetInterfaceKit_setDataRate(Handle, 6, 64);
	CPhidgetInterfaceKit_setDataRate(Handle, 7, 64);
	return 1;
};

void InterfaceBoard::Close() // Close connection
{
	#ifdef SHOW_DEBUG
	printf("Closing phidgets interface board.\n");
	#endif
	CPhidget_close((CPhidgetHandle)Handle);
};

int InterfaceBoard::GetSensorValue(unsigned int Index) // Get analog sensor value
{
	int Value;
	CPhidgetInterfaceKit_getSensorValue(Handle,Index,&Value);
	SensorChangeHandler(Handle, (void*)this, (int)Index, Value);
	return Sensors[Index]->GetValue();
};

int InterfaceBoard::GetInputState(unsigned int Index) // Get digital input state
{
	CPhidgetInterfaceKit_getInputState(Handle, Index, &OutputVals[Index]);
	return InputVals[Index];
};

int InterfaceBoard::GetOutputState(unsigned int Index) // Get digital output state
{
	CPhidgetInterfaceKit_getOutputState(Handle, Index, &OutputVals[Index]);
	return OutputVals[Index];
};

void InterfaceBoard::SetOutputState(unsigned int Index, int State) // Set digital output state
{
	OutputVals[Index] = State;
	CPhidgetInterfaceKit_setOutputState(Handle, Index, State);
};

void InterfaceBoard::UpdateSensors() // Forces sensors to update their values
{
	for(int i=0;i<8;i++)
	{
		Sensors[i]->Update(i, 0);
	} 
};

int InterfaceBoard::InputChangeHandler(CPhidgetInterfaceKitHandle hanlde, void *UserPtr, int Index, int State) // Input change handler
{
	((InterfaceBoard*)UserPtr)->InputVals[Index] = State;
	//((InterfaceBoard*)UserPtr)->wait(60);
	return 0;
};

int InterfaceBoard::OutputChangeHandler(CPhidgetInterfaceKitHandle handle, void *UserPtr, int Index, int State) // Output change handler
{
	((InterfaceBoard*)UserPtr)->OutputVals[Index] = State;
	//((InterfaceBoard*)UserPtr)->wait(60);
	return 0;
};

int InterfaceBoard::SensorChangeHandler(CPhidgetInterfaceKitHandle handle, void *UserPtr, int Index, int Value) // Sensor change handler
{
	((InterfaceBoard*)UserPtr)->Sensors[Index]->Update(Index, Value); 
	//((InterfaceBoard*)UserPtr)->wait(60);
	return 0;
};

// Sensor types for interface boardAttachHandlerAttachHandler 
void Sensor::Update(int Index, int Value)
{
	int Prev = value;
	value = Value;
	if (UpdateHandle!=0) UpdateHandle(value, Prev);
};

int Sensor::GetValue()
{
	return value;
};

Sensor::Sensor(CPhidgetInterfaceKitHandle boardHandle)
{
	value = 0;
	UpdateHandle = 0;
	BoardHandle = boardHandle;
};

Sensor::~Sensor()
{

};

Sonar::Sonar(CPhidgetInterfaceKitHandle boardHandle) : Sensor(boardHandle) 
{

};

void Sonar::Update(int Index, int Value)
{
	int Prev = value;
	CPhidgetInterfaceKit_getSensorRawValue(BoardHandle,Index,&Value);
	value = ( (int)((long long)Value * SONAR_CONV_RATIO_RAW) );
	_Index = Index; 
	if (UpdateHandle!=0) UpdateHandle(value, Prev);
};

IR::IR(CPhidgetInterfaceKitHandle boardHandle) : Sensor(boardHandle) 
{

};

void IR::Update(int Index, int Value)
{
	int Prev = value;
	CPhidgetInterfaceKit_getSensorRawValue(BoardHandle,Index,&Value);
	if(Value<327) Value=327;
	if(Value>2047) Value=2047;
	Value -= 82; 
	value = (19656/Value);
	if (UpdateHandle!=0) UpdateHandle(value, Prev);
};

// Encoder board
Encoder::Encoder()
{
	Handle = 0;
	CPhidgetEncoder_create(&Handle);
	CPhidget_set_OnAttach_Handler((CPhidgetHandle)Handle, AttachHandler, this);
	CPhidget_set_OnDetach_Handler((CPhidgetHandle)Handle, DetachHandler, this);
	CPhidget_set_OnError_Handler((CPhidgetHandle)Handle, ErrorHandler, this);
	CPhidgetEncoder_set_OnInputChange_Handler(Handle, Encoder::InputChangeHandler, this);
	CPhidgetEncoder_set_OnPositionChange_Handler (Handle, Encoder::PositionChangeHandler, this);
	Position=0;
};

Encoder::~Encoder()
{
	CPhidget_delete((CPhidgetHandle)Handle);
};

int Encoder::Open(int SerialNumber)
{
	int result;
	string* err;
	CPhidget_open((CPhidgetHandle)Handle, SerialNumber);
	if((result = CPhidget_waitForAttachment((CPhidgetHandle)Handle, 5000)))
	{
		#ifdef SHOW_ERROR
			CPhidget_getErrorDescription(result, (const char**)&err);
			printf("Problem waiting for attachment: %s\n", err);
		#endif
		return 0;
	}
	return 1;
};

void Encoder::Close()
{
	#ifdef SHOW_DEBUG
		printf("Closing phidgets encoder.\n");
	#endif
	CPhidget_close((CPhidgetHandle)Handle);
};

int Encoder::InputChangeHandler(CPhidgetEncoderHandle handle, void *UserPtr, int Index, int State)
{
	//printf("Encoder input changed\n");
	return 0;
};

int Encoder::PositionChangeHandler(CPhidgetEncoderHandle handle, void *UserPtr, int Index, int Time, int RelativePosition)
{
	//printf("Encoder %i: %i\n",Index, ((Encoder*)UserPtr)->Position);
	CPhidgetEncoder_getPosition(handle, Index, &(((Encoder*)UserPtr)->Position));
	return 0;
};

int Encoder::GetValue()
{
	return Position;
};

void Encoder::Reset()
{
	CPhidgetEncoder_setPosition(Handle, 0, 0);
};

// DC motor control boardAddMovementRecord 
DCMotor::DCMotor()
{
	CPhidgetMotorControl_create(&Handle);
	CPhidget_set_OnAttach_Handler((CPhidgetHandle)Handle, AttachHandler, this);
	CPhidget_set_OnDetach_Handler((CPhidgetHandle)Handle, DetachHandler, this);
	CPhidget_set_OnError_Handler((CPhidgetHandle)Handle, ErrorHandler, this);
	CPhidgetMotorControl_set_OnInputChange_Handler (Handle, InputChangeHandler, this);
	CPhidgetMotorControl_set_OnVelocityChange_Handler (Handle, VelocityChangeHandler, this);
	// Current change is not implemented on phidgets side
	CPhidgetMotorControl_set_OnCurrentChange_Handler (Handle, CurrentChangeHandler, this);
	for(int i=0;i<4;i++) InputVals[i]=0;
};

DCMotor::~DCMotor()
{
	CPhidget_delete((CPhidgetHandle)Handle);
};

void DCMotor::Close()
{
	// Stop before closing
	SetSpeed(0, 0, 100);
	SetSpeed(1, 0, 100);
	#ifdef SHOW_DEBUG
		printf("Closing phidgets DC motor board.\n");
	#endif
	CPhidget_close((CPhidgetHandle)Handle);
};

int DCMotor::Open(int SerialNumber)
{
	int result;
	string *err;
	CPhidget_open((CPhidgetHandle)Handle, SerialNumber);
	
	if((result = CPhidget_waitForAttachment((CPhidgetHandle)Handle, 5000)))
	{
		#ifdef SHOW_ERROR
			CPhidget_getErrorDescription(result, (const char**)&err);
			printf("Problem waiting for attachment: %s\n", err);
		#endif
		return 0;
	}
	return 1;
};

int DCMotor::InputChangeHandler(CPhidgetMotorControlHandle handle, void *UserPtr, int Index, int State)
{
	((DCMotor*)handle)->InputVals[Index] = State;
	return 0;
};

int DCMotor::VelocityChangeHandler(CPhidgetMotorControlHandle handle, void *UserPtr, int Index, double Value)
{
	return 0;
};

int DCMotor::CurrentChangeHandler(CPhidgetMotorControlHandle handle, void *UserPtr, int Index, double Value)
{
	return 0;
};

void DCMotor::SetSpeed(int MotorIndex, int Speed, int Acceleration)
{
	CPhidgetMotorControl_setAcceleration (Handle, MotorIndex, Acceleration);
	CPhidgetMotorControl_setVelocity (Handle, MotorIndex, Speed);
};

int DCMotor::GetInputState(unsigned int Index) // Get digital input state
{
	return InputVals[Index];
};

// Servo control board
Servo::Servo()
{
	CPhidgetAdvancedServo_create(&Handle);
	CPhidget_set_OnAttach_Handler((CPhidgetHandle)Handle, AttachHandler, this);
	CPhidget_set_OnDetach_Handler((CPhidgetHandle)Handle, DetachHandler, this);
	CPhidget_set_OnError_Handler((CPhidgetHandle)Handle, ErrorHandler, this);
	CPhidgetAdvancedServo_set_OnPositionChange_Handler(Handle, PositionChangeHandler, this);
	Position = 0;
};

Servo::~Servo()
{
	CPhidget_delete((CPhidgetHandle)Handle);
};

void Servo::Close()
{
	CPhidgetAdvancedServo_setEngaged (Handle, 0, 0);
	#ifdef SHOW_DEBUG
		printf("Closing servo control board.\n");
	#endif
	CPhidget_close((CPhidgetHandle)Handle);
};

int Servo::Open(int SerialNumber)
{
	int result;
	string *err;
	double minAccel, maxVel;
	CPhidget_open((CPhidgetHandle)Handle, SerialNumber);
	if((result = CPhidget_waitForAttachment((CPhidgetHandle)Handle, 10000)))
	{
		#ifdef SHOW_ERROR
			CPhidget_getErrorDescription(result, (const char**)&err);
			printf("Problem waiting for attachment: %s\n", err);
		#endif
		return 0;
	}
	
	//Set up some initial acceleration and velocity values
	CPhidgetAdvancedServo_getAccelerationMin(Handle, 0, &minAccel);
	CPhidgetAdvancedServo_setAcceleration(Handle, 0, minAccel*20);
	CPhidgetAdvancedServo_getVelocityMax(Handle, 0, &maxVel);
	CPhidgetAdvancedServo_setVelocityLimit(Handle, 0, maxVel*3);

	CPhidgetAdvancedServo_setEngaged (Handle, 0, 1);
	return 1;
};

int Servo::PositionChangeHandler(CPhidgetAdvancedServoHandle handle, void *UserPtr, int Index, double Value)
{
	((Servo*)UserPtr)->Position = (int)Value;
};

void Servo::SetPosition(int Position)
{
	CPhidgetAdvancedServo_setEngaged (Handle, 0, 1);
	CPhidgetAdvancedServo_setPosition (Handle, 0, Position);
};

int Servo::GetPosition()
{
	return Position;
};

// Device manager 

Manager::Manager()
{
	//create the Manager object
	CPhidgetManager_create(&Handle);

	//Set the handlers to be run when the device is plugged in or opened from software, unplugged or closed from software, or generates an error.
	CPhidgetManager_set_OnAttach_Handler(Handle, AttachHandler, this);
	CPhidgetManager_set_OnDetach_Handler(Handle, DetachHandler, this);
	CPhidgetManager_set_OnError_Handler(Handle, ErrorHandler, this);
};

Manager::~Manager()
{
	CPhidgetManager_delete(Handle);
};

int Manager::Open()
{
	CPhidgetManager_open(Handle);
	return 0;
}

void Manager::Close()
{
	CPhidgetManager_close(Handle);
}

int Manager::AttachHandler(CPhidgetHandle Handle, void *UserPtr)
{
	((Manager*)UserPtr)->UpdateDeviceList();
	return 0;
};

int Manager::DetachHandler(CPhidgetHandle Handle, void *UserPtr)
{
	((Manager*)UserPtr)->UpdateDeviceList();
	return 0;
};

int Manager::ErrorHandler(CPhidgetManagerHandle Handle, void *UserPtr, int Code, const char *Description)
{
	#ifdef SHOW_ERROR
		printf("Device manager error handled:\n%d - %s\n", Code, Description);
	#endif
	return 0;
};

void Manager::UpdateDeviceList()
{
	CPhidgetHandle* devices;
	CPhidgetManager_getAttachedDevices (Handle, &devices, &DeviceCount);
	memcpy(&DeviceHandles[0],devices,DeviceCount*8);
};

void Manager::GetDeviceDescription(CPhidgetHandle handle, CPhidget_DeviceClass* Class, CPhidget_DeviceID* ID, int* SerialNumber, string& Type)
{
	CPhidget_getDeviceClass(handle, Class);
	CPhidget_getDeviceID(handle, ID);
	CPhidget_getSerialNumber(handle, SerialNumber);
	const char* devtype;
	CPhidget_getDeviceType(handle, &devtype);
	Type=string(devtype);
};

void Manager::GetDevices(CPhidgetHandle* deviceHandles, int* Count)
{
	
	memcpy(deviceHandles,&DeviceHandles[0], DeviceCount*8);
	*Count=DeviceCount;
};
